#include "../_includes.h"

void auto_left_1() {
  gyro_reset();
  turn_pid_adjust_agl_absolute(90,100,800,Block::yes);
  // chassis.stable_forward_dist_relative(-150, 100, 800, Block::yes);
  // wing_lock.set(true);
  // wait(200);
  // chassis.stable_forward_dist_relative(160, 100, 800, Block::yes);
  // // chassis.FT_set_speed(100,100);
  // // chassis_r.act_for_enc_relative(350, 100, 1000);

  // turn_pid_adjust_agl_absolute(-200, 50, 1000, Block::yes);
  // wing_lock.set(false);
  // chassis.stable_forward_dist_relative(485, 100, 800, Block::yes);



  // turn_pid_adjust_agl_absolute(-160, 100, 800, Block::yes);
  // chassis.stable_forward_dist_relative(480, 100, 850, Block::yes);
  // chassis.stable_forward_dist_relative(-300, 100, 800, Block::yes);

  // turn_pid_adjust_agl_absolute(-200, 100, 1000, Block::yes);

  // chassis.stable_forward_dist_relative(-790, 100, 1500, Block::yes);

  // turn_pid_adjust_agl_absolute(-251, 100, 1000, Block::yes);
  // chassis.stable_forward_dist_relative(-790, 100, 1500, Block::yes);
  // wait(1000000);

  // chassis.stable_forward_dist_relative(460, 100, 800, Block::yes);
  // turn_pid_adjust_agl_absolute(45, 100, 500, Block::yes);
  // chassis.stable_forward_dist_relative(600,100,1000,Block::yes);
  // chassis.stable_forward_dist_relative(-370,100,800,Block::yes);
}

// //正面网放预装；挖角落粽子；推通道粽子

// void auto_left_1() {
//   dig.act_for_enc_absolute(enc_dig_upright, 100, 1000, Block::no); //解锁
//   //走到中线
//   chassis.stable_forward_dist_relative(980, 100, 1000, Block::yes);
//   wait(200);
//   turn_pid_adjust_agl_absolute(-90, 100, 1000, Block::yes); //转向网子

//   intake.set_voltage(-100);
//   wait(200, timeUnits::msec); //等粽子掉下来
//   intake.set_voltage(20);

//   chassis.fast_forward_dist_relative(200, 100, 1500,
//                                      Block::yes); //走到网，粽子推进去

//   wait(300);
//   chassis.stable_forward_dist_relative(-180, 100, 1500, Block::yes); //后退
//   wait(100);
//   turn_pid_adjust_agl_absolute(-180, 100, 1000, Block::yes); //左转向
//   chassis.stable_forward_dist_relative(650, 100, 1500,
//                                        Block::yes); //前进至转弯处
//   turn_pid_adjust_agl_absolute(-125, 100, 1000, Block::yes); //转向
//   wait(100);
//   intake.mtr_set_voltage(100); //提前开启滚筒
//   dig.act_for_enc_absolute(enc_dig_prepare_to_cellect_triball, 100, 1000,
//                            Block::yes); // dig提前就位
//   chassis.stable_forward_dist_relative(650, 100, 1500,
//                                        Block::yes); //前进至三角区域
//   chassis.FT_set_voltage(5, 0);                     //抵住杆子
//   dig.act_for_enc_absolute(enc_dig_collect_triball, 100, 1000,
//                            Block::yes);                              //挖粽子
//   dig.stop(hold);                                                    //稳住
//   chassis.stable_forward_dist_relative(-250, 100, 1500, Block::yes); //后退
//   chassis.stable_forward_dist_relative(50, 100, 1500, Block::yes);
//   wait(500);
//   dig.act_for_enc_absolute(enc_dig_prepare_hit_hang_stick, 100, 1000,
//                            Block::no);                       //挖挖回位
//   turn_pid_adjust_agl_absolute(-225, 100, 1000, Block::yes); //转向
//   wait(100);
//   chassis.stable_forward_dist_relative(350, 100, 1500,
//                                        Block::yes); //前进一点到接近挂杆
//   intake.stop();
//   turn_pid_adjust_agl_absolute(-272, 100, 1000, Block::yes); //转向挂杆
//   toss.act_for_enc_absolute(95, 100, 1000,
//                             Block::no); //弹射辅助防止过线
//   wait(100);
//   intake.mtr_set_voltage(-100); //吐粽子
//   chassis.stable_forward_dist_relative(825, 100, 1500,
//                                        Block::yes); //前进，并碰杆
//   cout << "auctual time:" << t_auto.time() << endl;
//   while_wait(t_auto.time() < 15000);
// }
